//
// Created by lee on 24-6-26.
//

#include "threads.h"
#include "struct_define.h"
#include <iostream>
#include <thread>
#include "geo/geo.h"

void send_routes_packet(const char *ipAddr, int port) {
    const int num = 3;

    uint8_t send_buffer[1024];
    int sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
    struct sockaddr_in serverAddr{};
    socklen_t addrLen = sizeof(serverAddr);
    // 设置服务器地址和端口
    serverAddr.sin_family = AF_INET;
    serverAddr.sin_port = htons(port);
    serverAddr.sin_addr.s_addr = inet_addr(ipAddr);

    uint8_t msg_head[] = {0x55, 0xAa, 0X00, 25+22*num, 0x2E};
    ROUTE_INFO routeInfo{};

    routeInfo.total_number_of_waypoints_in_this_package = num;
    int lat_int = 32.563120*1e7;
    int lon_int = 116.724818*1e7;
    int height_int = 200; //不用倍数
    routeInfo.routes[0] = agx_2px4_route_single_t{1, 2,lon_int+10000, lat_int+10, height_int};
    routeInfo.routes[1] = agx_2px4_route_single_t {4,5, lon_int+20000, lat_int+20, height_int};
    routeInfo.routes[2] = agx_2px4_route_single_t {7,8, lon_int+30000, lat_int+30, height_int};
    uint8_t tmp[25+22*num] = {0};
    memcpy(tmp, msg_head, 5);
    memcpy(&tmp[5], &routeInfo, 19+ 22*routeInfo.total_number_of_waypoints_in_this_package);

    int check_sum = 0;
    for(int i = 0; i<22*num+24; ++i){
        check_sum += tmp[i];
    }
    tmp[22*num+24] = (check_sum & 0xff);

    while(true){
        int send_res = sendto(sock_fd, tmp, 22*num + 25, 0, (sockaddr *) &serverAddr, addrLen);
//        std::cout<<"send_res: "<<send_res<<std::endl;
//        if(send_res > 0) std::cout<<"success"<<std::endl;
        usleep(1000*100);
    }

}

void print_test(){
    while(true){
        std::cout<<"test"<<std::endl;
        usleep(1000*100);
    }
}

int main() {
    std::thread send_test_thread(send_routes_packet, "127.0.0.1", 8001);

//    int receivePort = 26540;
//    int sendPort = 24580;

    int receivePort = 14550;
    int sendPort = 18570;
    char* ipAddr = "127.0.0.1";//目标IP地址

    int sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    struct sockaddr_in serverAddr{};
    socklen_t addrLen = sizeof(sockaddr_in);
    // 设置服务器地址和端口
    serverAddr.sin_family = AF_INET;
    serverAddr.sin_addr.s_addr = inet_addr("127.0.0.1");
    serverAddr.sin_port = htons(receivePort);
    //接受程序绑定端口
    int bind_res = bind(sockfd, (struct sockaddr *) &serverAddr, addrLen);
    if (bind_res < 0) perror("bind error");

//    //初始化全局坐标系
//    double global_init[2] = {32.5634972, 116.7253924};
//    float global_init_h = 0;
//    globallocalconverter_init(global_init[0], global_init[1], global_init_h, 0);
    thread udp_recv_thread(udp_receive, ipAddr, 8001);
    thread mavlink_send_thread(mavlink_send, sockfd, ipAddr, sendPort);
    thread mavlink_recv_thread(mavlink_receive, sockfd, ipAddr, receivePort);

    udp_recv_thread.join();
    mavlink_send_thread.join();
    mavlink_recv_thread.join();
    send_test_thread.join();
    return 0;
}
